
#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <ros/time.h>
#include "can-ctrl.h"
#include "zeus_s3_driver/controlcan.h"
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TransformStamped.h>
#include <dynamic_reconfigure/server.h>
#include <tf/tf.h>
#include "sensor_msgs/JointState.h"
#include "zeus_s3_msgs/Feedback.h"
#include "zeus_s3_msgs/wheel_state.h"
#include "zeus_s3_msgs/motor_pid_vol.h"
#include "zeus_s3_msgs/joy_control.h"
#include <zeus_s3_driver/PID_reconfigConfig.h> 
#include <cmath>
#include <algorithm>
#include <math.h>
// #include "zeus_s2_driver/my_timer.h"

/*  使用高配版nvidia-xavier主机，自带can通信*/
 #define NVIDIA_XAVIER    1

#define MAX_LINEAR  1.57        //m/s
#define MAX_ANGULAR  0.7853     //red/s

#define GROUP_INT_BYTE(X,N)   ((X[N+3]<<24)+(X[N+2]<<16)+(X[N+1]<<8)+X[N])
#define  MOTOR_ID(X,N)   ((X[N]<<8 | X[N+1])>>5)

#define PID_INTEGRAL_UP    0.8   //积分上限
#define PID_SCALE      1.0     //PID缩放系数
#define PID_TEMP        1.5

#define MAX      1270.0        //32767
#define MIN      -1270.0        //-32768

#define PI                        3.1415926
#define  DI                      458936.6552493    // (11*16384)/(22.5*(3.14/180))         180224/0.392699075==458936.6552493

enum _enum_DirveID{

        DRIVER_OPERATION_MODE = 1,
        DRIVER_TIMEOUT_DETECTION = 2,
        DRIVER_PARKING_MODE = 3,
        DRIVER_SWITCHING_MODE = 0,

        DIRVEE_ID_SYSTEM_STATE_ACK = 0x151,
        DIRVEE_ID_MOVE_CONTROL_ACK = 0x131,
        DIRVEE_ID_MOVE_CONTROL = 0x130,
        DIRVEE_ID_ONE_MOTOR_ACK = 0x201,
        DIRVEE_ID_TWO_MOTOR_ACK = 0x202,
        DIRVEE_ID_THREE_MOTOR_ACK = 0x203,
        DIRVEE_ID_FOUR_MOTOR_ACK = 0x204,
        DIRVEE_ID_LAMPLIGHT_CONTROL = 0x140,		//lamplight
        DIRVEE_ID_LAMPLIGHT_CONTROL_ACK = 0x141,

        DRIVER_ID_ONE_POSE_ACK = 0X601,
        DRIVER_ID_TWO_POSE_ACK = 0X602,
        DRIVER_ID_THREE_POSE_ACK = 0X603,
        DRIVER_ID_FOUR_POSE_ACK = 0X604,

        DRIVER_ID_ONE_CONTROL = 0x101,
        DRIVER_ID_TWO_CONTROL = 0x102,
        DRIVER_ID_THREE_CONTROL = 0x103,
        DRIVER_ID_FOUR_CONTROL = 0x104,

        DRIVER_ID_MODE_ACK = 0X700,
 
};

enum  _enum_timeManagement{

  TIME_DRIVER_MODE=0,
   TIME_SUM,
 
};

struct  DIRVEDATA
{
   uint8_t carBodyStateAck;
   int8_t  modeControlAck;
   uint16_t batteryVoltageAck;
   uint16_t faultMessageAck;

   int16_t movementSpeedAck;
   int16_t rotationalSpeedAck;
	
   uint8_t controlMode;
   uint8_t faultClearance;
   int8_t  percentLinear;
   int8_t  percentAngular;
   
   uint16_t oneMotorElectricCurrentAck;
   int16_t oneMotorRorateSpeedAck;
   int8_t  oneMotorTemperatureAck;
   
   uint16_t twoMotorElectricCurrentAck;
   int16_t twoMotorRorateSpeedAck;
   int8_t  twoMotorTemperatureAck;

   uint16_t threeMotorElectricCurrentAck;
   int16_t threeMotorRorateSpeedAck;
   int8_t  threeMotorTemperatureAck;

   uint16_t fourMotorElectricCurrentAck;
   int16_t fourMotorRorateSpeedAck;
   int8_t  fourMotorTemperatureAck;

   uint8_t lightControlEnableSign;
   uint8_t bodyLightPattern;

   uint8_t currentLightControlEnableSignAck;
   uint8_t bodyLightPatternAck;
} ;

class DriverPub
{
   public:
        DriverPub();
        ~DriverPub();
        void loop();
    private:
        bool initRobot();
        void receive_func();
        void send_speed_callback(void);
        void handle_speed_data(uint16_t number);
        void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg);
        void enable_motor_driver(uint16_t driver_id );
        void disable_motor_driver(uint16_t driver_id );
        void clear_ALM_motor_driver(uint16_t driver_id );
        void Set_motor_torque_mode(uint16_t driver_id );
        void Set_torque_value_to_motor(uint16_t driver_id ,int16_t value );
        void Read_motor_status_infor(uint16_t driver_id );
        void Read_motor_electricity_infor(uint16_t driver_id );
        void Set_motor_absolute_location(uint16_t driver_id ,int vol);
        void Calculate_turn_execution_time(void);

#ifdef  NVIDIA_XAVIER
         int step=0;
	      char tx_buf[8] = {0, 0x1E, 0, 0x03, 0, 1, 0, 0};
	      char rx_buf[8] = {0, 0, 0, 0, 0, 0, 0, 0};
        struct can_frame can_id;
        void Calculate_LeftAndRight_Meter(struct can_frame *p);
#else

        VCI_BOARD_INFO pInfo;//用来获取设备信息。
        VCI_BOARD_INFO pInfo1 [50];
        void dirve_ProcessData(VCI_CAN_OBJ *p,DIRVEDATA *PdirveDate );
        void Calculate_LeftAndRight_Meter(VCI_CAN_OBJ *p);

#endif
      
        uint8_t get_recvFlag(void);
        int16_t  motor_velocityPidCtl_one(float target_speed,float current_speed);
        int16_t  motor_velocityPidCtl_two(float target_speed,float current_speed);
        int16_t  motor_velocityPidCtl_three(float target_speed,float current_speed);
        int16_t  motor_velocityPidCtl_four(float target_speed,float current_speed);
        void dynamic_reconfig_callback(zeus_s3_driver::PID_reconfigConfig &config,uint32_t level);
        void motor_driver_check(void);
      
      
        boost::mutex cmd_vel_mutex_;
        ros::Time last_twist_time_;
        
        boost::mutex mutex_;

        ros::Subscriber cmd_sub_;
        ros::Subscriber cmd_joy_moment_;
        ros::Publisher odom_pub_;
        ros::Publisher wheel_state_feedback_;
        zeus_s3_msgs::Feedback  left_right_wheel_state_;
        ros::Publisher speed_pid_;              //发布测试pid话题
        zeus_s3_msgs::motor_pid_vol  motor_pid_[4];      //调试pid参数类型

        ros::Timer timer1;                                  //ros的定时器
    

        std::string code_version_;
        std::string port_name_;

        std::string odom_frame_;
        std::string base_frame_;

         
        bool recv_flag_;        //用于1号电机回零标志位
        bool start_flag_;
        bool publish_odom_transform_;
        bool joy_flag_=0;
        geometry_msgs::Twist current_twist_;
        nav_msgs::Odometry odom_;
        geometry_msgs::TransformStamped transformStamped_;
         tf2_ros::TransformBroadcaster br_;

        int driver_count;

        int fdcom;
        int baud_rate_;
        unsigned char buffer_data[100];

        int ret;
        // pthread_t threadid;
        ros::Time now_[4];
        ros::Time last_time_[4];
       
        // short error;

        double wheel_track_4wd_;
        double wheel_track_;
        double encoder_resolution_4wd_;
        double ticks_per_meter_;       //脉冲数/m

        double linear_correction_factor_;
        double angular_correction_factor_;
        double gear_reduction_4wd_;     //减速比
        double wheel_diameter_4wd_;     //轮子直径
        double start_rotation_limit_w_;
        double delta_time_[4];
        double delta_xy_ave_;
        double delta_th_;
        double v_linear_;
        double v_angular_;
        double delta_x_;
        double delta_y_;

        double accumulation_x_;
        double accumulation_y_;
        double accumulation_th_;
        double accumulation_left_;
        double accumulation_left_1_;
        double accumulation_right_;
        double accumulation_right_1_;

        double delta_left_;
        double delta_right_;        //里程差值 单位 m
        double delta_left_1_;
        double delta_right_1_;        //里程差值 单位 m
        int32_t rev_left_;
        int32_t rev_left_1_;
        int32_t rev_right_;       
        int32_t rev_right_1_;
        int cur_left_;              //存放脉冲计数值
        int cur_left_1_; 
        int cur_right_;
        int cur_right_1_;
        double encoder_low_wrap_;       //解决编码器溢出
        double encoder_high_wrap_;
        int l_wheel_mult_;              //多圈计数
        int l_wheel_mult_1_;              //多圈计数
        int r_wheel_mult_;
        int r_wheel_mult_1_;
        int encoder_min_;
        int encoder_max_;   

        int32_t line_speed_percent;
        int32_t angle_speed_percent;

        uint8_t queueFlag;             //用于通信检测标志
        uint8_t can_recv_flag;

        float motor_kp[4];
        float motor_ki[4];
        float motor_kd[4];
        int ele_currVol;
        float speed_l;
        float speed_r;
        short error;


        int16_t  motor_ctlVol_one;
        int16_t  motor_ctlVol_two;
        int16_t  motor_ctlVol_three;
        int16_t  motor_ctlVol_four;
        float left_des;
        float right_des;
        bool  reset_vol;
        bool  mode_selection;
        int     pid_wheel_;
        float  left;
        float   right;

        float  motorEleVol[4]={0};      //驱动器反馈电流值
        int16_t     motorSpeed[4] = {0};       //电机的当前速度
        int16_t     motorTemperature[4] = {0}; //电机温度
        int     driverFaultCode[4] = {0}; //驱动器故障码
        uint8_t  rev_flag;                //接收驱动器信息并处理标志
        uint64_t count = 0;                               //根据频率定时判断驱动器通信问题
        float  bias[4] = {0};
        float bias_last[4] = {0};
        float bias_integral[4] = {0};
        uint8_t mode_rev_flag = 0;            //用于判断模式切换是否成功
        bool  mode_isnot = 0;
        uint8_t switching_mode = 0;
        u_int64_t  timeArray[2] = {0};        //用于计时数组
        uint8_t state_temp_  =0;

        float car_wheelbase = 0;      //轴距
        float maxsteerInside = 0;        //最大角度
        float rMax = 0;                                 //最大角度对应的半径
        float rideal = 0;                               //车辆中心转弯半径
        float maxsteer = 0;                        //最大中心转角（下发的角度不能大于此角度）
        float angular_w =0;                      //记录转弯角度
        double linear_speed = 0; 
        double angular_speed = 0;
        double angular_z =0;                   //临时记录转弯角度
        double x = 0;
        float r_left = 0;
        float r_right = 0;
        float r = 0;
        int time_angular = 0;                   //转弯执行时间
        float last_angular_w = 0;               //上一次转弯位置

};

